﻿using CrankShaftAnalysisSystem.Controller;
using CrankShaftAnalysisSystem.Devices;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace CrankShaftAnalysisSystem
{
    public class LinearController
    {
        public static double Kp { get; set; } = 0.7;
        public static short Acc { get; set; } = 150;  // 加速rpm

        public static short Decc { get; set; } = 150; // 减速rpm
        public static int VelMin { get; private set; } = 50; // 最小速度
        public static int VelMax { get; private set; } = 1500;
        public static double FowardFeed { get; set; } = 1.6; // 前馈
        public static double MaxHeight { get; set; } = 81500; // 最大高度
        public static double MinHeight { get; set; } = 31500; // 最小高度

        public const int Axis = 2; // 2号电机控制高度

        public static MeasureSystem.State State { get; private set; } = MeasureSystem.State.IDLE;
        
        /// <summary>
        /// 设定高度，RV速比79，左边丝杠导程5mm，右边10mm
        /// </summary>
        /// <param name="set_dis">高度位置，单位um</param>
        /// <param name="tol">误差um</param>
        /// <returns></returns>
        public static Task SetPosition(double set_dis, double tol = 5)
        {
            if (State == MeasureSystem.State.BUSY)
            {
                throw new Exception("Linear Controller Busy");
            }
            State = MeasureSystem.State.BUSY;
            if (set_dis > MaxHeight || set_dis < MinHeight)
            {
                throw new Exception($"[Linear Controller Error]: {set_dis} Not Available.");
            }
            LinearLC485 LC485 = LinearLC485.Instance;
            PLCMaster PLC = PLCMaster.Instance;
            int vel_setpoint = 0;
            int vel_tmp = 0;
            double brake_dis = 0;
            double acc_umps = Acc / 60.0 / 79.0 * 5000.0; // rpm / 60 = rps, rps / 减速比 = 圈/s, 圈/s * 导程 = [um/s]

            return Task.Run(() =>
            {
                while (true)
                {
                    var current_height = LC485.ReadLinearDis();
                    double brake_time = 0.1 * vel_setpoint / Acc;
                    brake_dis = 0.5 * acc_umps * brake_time * brake_time;
                    var pos_err = set_dis - (current_height + FowardFeed * brake_dis);

                    if (Math.Abs(current_height - set_dis) < tol)
                    {
                        Console.WriteLine("Position Reached.");
                        PLC.SetSpeed(Axis, 0);
                        break;
                    }

                    if (SafeController.ManualStop)
                    {
                        Console.WriteLine("[Safe Controller]: StopAll.");
                        PLC.SetSpeed(Axis, 0);
                        break;
                    }

                    vel_tmp = (int)(Kp * pos_err);
                    if (Math.Abs(vel_tmp) < VelMin) vel_tmp = Math.Sign(vel_tmp) * VelMin;
                    if (vel_setpoint < vel_tmp)
                    {
                        vel_setpoint += Decc;
                        if (vel_setpoint > vel_tmp) vel_setpoint = vel_tmp;
                        if (vel_setpoint > VelMax) vel_setpoint = VelMax;
                    }
                    if (vel_setpoint > vel_tmp)
                    {
                        vel_setpoint -= Acc;
                        if (vel_setpoint < vel_tmp) vel_setpoint = vel_tmp;
                        if (vel_setpoint < -VelMax) vel_setpoint = -VelMax;
                    }
                    PLC.SetSpeed(Axis, vel_setpoint);
                    Thread.Sleep(150);
                }
                State = MeasureSystem.State.FINISHED;
            });
        }
    }
}
